
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/PoseStamped.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Odometry.h>
#include "opencv2/opencv.hpp"
// #include <zbar.h> // zbar库 sudo apt-get install libzbar0 libzbar-dev
#include <iostream>
#include <string>
#include <vector>
#include <chrono>
#include <thread>
#include <math.h>

using namespace std;
using namespace cv;

double time_between_tolerance = 3.0; // 距离上次检测到的时间后没有检测到同样条码的最大间隔 s
double time_last = 0.0;              // 上次检测时间

double ground_truth_x_where_last_shot = 0.0;
double ground_truth_y_where_last_shot = 0.0;
double ground_truth_x = 0.0;
double ground_truth_y = 0.0;
double stick_truth_x = 0.0;
double stick_truth_y = 0.0;

cv::Mat current_frame;
bool new_frame = false;

void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        current_frame = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
        new_frame = true;
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }
}

void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
    ground_truth_x = msg->pose.pose.position.x + stick_truth_x;
    ground_truth_y = msg->pose.pose.position.y + stick_truth_y;
}

void stickCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    stick_truth_x = msg->pose.position.x;
    stick_truth_y = msg->pose.position.y;
}

double blue_x = 0.5; //蓝色物体图像的位置，左0右1
double blue_y = 0.5; //蓝色物体图像的位置，上0下1
int blue_count = 0;

int is_blue_found_then_move_above(cv::Mat &frame) //这里是蓝色
{
    Mat hsv, mask_blue;
    cvtColor(frame, hsv, COLOR_BGR2HSV);
    Scalar lower_blue(100, 214, 35);
    Scalar upper_blue(165, 255, 255);
    inRange(hsv, lower_blue, upper_blue, mask_blue);

    Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(5, 5));
    // 先进行腐蚀操作去除噪点
    erode(mask_blue, mask_blue, kernel, Point(-1, -1), 1);
    // 再进行膨胀操作恢复并增强轮廓
    dilate(mask_blue, mask_blue, kernel, Point(-1, -1), 10);

    // 寻找轮廓
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(mask_blue, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

    // 创建mask_blue的副本并转换为三通道图像以便画轮廓
    Mat mask_display;
    cvtColor(mask_blue, mask_display, COLOR_GRAY2BGR);
    
    // 在副本上画出所有轮廓
    int best_contour_index = -1;
    double min_distance_to_center = DBL_MAX;
    double best_center_x = 0.5;
    double best_center_y = 0.5;
    
    // 画面中心坐标
    double image_center_x = frame.cols / 2.0;
    double image_center_y = frame.rows / 2.0;
    
    for (int i = 0; i < contours.size(); i++)
    {
        // 用不同颜色画轮廓，绿色轮廓线，厚度为2
        drawContours(mask_display, contours, i, Scalar(0, 255, 0), 2);
        
        // 可选：画轮廓的外接矩形
        Rect boundingRect = cv::boundingRect(contours[i]);
        rectangle(mask_display, boundingRect, Scalar(255, 0, 0), 1);
        
        // 计算最小外接圆
        Point2f circle_center;
        float circle_radius;
        minEnclosingCircle(contours[i], circle_center, circle_radius);
        double circle_area = CV_PI * circle_radius * circle_radius;
        
        // 计算最小外接矩形
        RotatedRect min_rect = minAreaRect(contours[i]);
        double min_rect_area = min_rect.size.width * min_rect.size.height;
        
        // 计算最小外接圆和最小外接矩形的面积比值
        double circle_to_rect_ratio = circle_area / min_rect_area;
        
        // 检查是否符合面积比条件 [0.8-1.3]
        if (circle_to_rect_ratio >= 0.8 && circle_to_rect_ratio <= 1.3)
        {
            // 计算重心（质心）
            Moments moments = cv::moments(contours[i]);
            double cx = moments.m10 / moments.m00; // 重心x坐标
            double cy = moments.m01 / moments.m00; // 重心y坐标
            
            // 计算与画面中心的距离
            double distance_to_center = sqrt(pow(cx - image_center_x, 2) + pow(cy - image_center_y, 2));
            
            // 如果距离更近，更新最佳轮廓
            if (distance_to_center < min_distance_to_center)
            {
                min_distance_to_center = distance_to_center;
                best_contour_index = i;
                best_center_x = cx / frame.cols; // 归一化坐标
                best_center_y = cy / frame.rows; // 归一化坐标
            }
        }
    }
    
    int is_blue_found = 0; // 是否检测到蓝色物体
    double percentage_glb = 0.0; // 目标像素占比
    // 如果找到符合条件的轮廓，打印位置信息
    if (best_contour_index != -1)
    {
        // cout << "找到最佳轮廓 " << best_contour_index << ":" << endl;
        // cout << "  重心位置: (" << best_center_x << ", " << best_center_y << ")" << endl;
        // cout << "  距离中心: " << min_distance_to_center << " 像素" << endl;
        
        // 获取最佳轮廓的外接矩形
        Rect bounding_rect = cv::boundingRect(contours[best_contour_index]);
        
        // 确保边界框在图像范围内
        int roi_x = max(0, bounding_rect.x);
        int roi_y = max(0, bounding_rect.y);
        int roi_width = min(frame.cols - roi_x, bounding_rect.width);
        int roi_height = min(frame.rows - roi_y, bounding_rect.height);
        
        if (roi_width > 0 && roi_height > 0) {
            // 提取最小外接矩形区域的图像
            Rect roi_rect(roi_x, roi_y, roi_width, roi_height);
            Mat roi_image = frame(roi_rect);
            
            // 转换ROI区域到HSV
            Mat roi_hsv;
            cvtColor(roi_image, roi_hsv, COLOR_BGR2HSV);
            
            // 定义RGB范围 - 查找白色像素
            Scalar lower_rgb(254, 254, 254);
            Scalar upper_rgb(255, 255, 255);
            
            // 创建掩膜（直接在BGR色彩空间中操作）
            Mat rgb_mask;
            inRange(roi_image, lower_rgb, upper_rgb, rgb_mask);
            
            // 计算符合条件的像素点数
            int target_pixels = countNonZero(rgb_mask);
            int total_pixels = roi_width * roi_height;
            double percentage = (double)target_pixels / total_pixels * 100.0;
            // 打印分析结果
            // cout << "  ROI区域分析:" << endl;
            // cout << "    ROI尺寸: " << roi_width << "x" << roi_height << endl;
            // cout << "    RGB范围(254,254,254)-(255,255,255)像素数: " << target_pixels << endl;
            // cout << "    总像素数: " << total_pixels << endl;
            // cout << "    目标像素占比: " << percentage << "%" << endl;
            
            // 显示ROI区域图像
            imshow("ROI Image", roi_image);
            // 显示RGB掩膜
            imshow("RGB Mask", rgb_mask);

            percentage_glb = percentage; // 更新全局变量
        }

        int found_now = 0;
        if (percentage_glb < 0.15) // 如果目标像素占比小于0.15%，则认为没有找到蓝色物体
        {
            // cout << "目标像素占比过低，未找到蓝色物体" << endl;
            // return 0; // 没有找到蓝色物体
        }else{
            cout << "目标像素占比: " << percentage_glb << "%" << endl;
            found_now = 1;
            blue_count++;
        }
        // 更新全局变量
        blue_x = best_center_x;
        blue_y = best_center_y;
        if (blue_x < 0.0) blue_x = 0.0; // 确保在0-1范围内
        if (blue_count > 7){
            // blue_count = 0; // 重置计数
            cout << "蓝色物体检测次数超过7次，认为已找到蓝色物体" << endl;
            is_blue_found = 1;
        }else if (blue_count != 0 && !found_now){
            blue_count = 0; // 重置计数
        }
        // 在最佳轮廓上画特殊标记
        drawContours(mask_display, contours, best_contour_index, Scalar(0, 0, 255), 3); // 红色粗线
    }
    
    // 显示轮廓图像
    imshow("Blue Mask with Contours", mask_display);

    return is_blue_found;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "qr_code_scanner");
    ros::NodeHandle nh;

    ros::Subscriber image_sub = nh.subscribe("/usb_cam/image_raw", 1, imageCallback);
    ros::Subscriber location_sub = nh.subscribe("/Odometry", 1, odomCallback);
    ros::Subscriber stick_location_sub = nh.subscribe("/stick_location", 1, stickCallback);
    ros::Publisher hold_pub = nh.advertise<geometry_msgs::PoseStamped>("/fire", 10);

    ros::Rate loop_rate(60);

    while (ros::ok())
    {
        ros::spinOnce(); // 处理回调

        // 只有在收到新帧时才处理
        if (new_frame)
        {
            new_frame = false;

            cv::Mat frame = current_frame.clone();
            if (frame.empty())
            {
                ROS_ERROR("Empty frame");
                loop_rate.sleep();
                continue;
            }

            if (1)
            {
                cv::Mat gray;
                cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
            }
            
            double st_time = ros::Time::now().toSec();
            int is_blue_found = is_blue_found_then_move_above(frame);
            if (is_blue_found)
            {
                cout << "blue found" << endl;      
                time_last = ros:: Time::now().toSec();
            }
            double end_time = ros::Time::now().toSec();
            cout << "Time taken for processing: " << (end_time - st_time)*1000 << " ms" << endl;
            cv::imshow("Camera Feed", frame);
            if (cv::waitKey(1) == 27)
            {
                break;
            }
//------------------------------------------------------
//-----------------------数据处理-------------------------
//------------------------------------------------------
            if (ros::Time::now().toSec() - time_last > time_between_tolerance)
            {
                // is_bar_found_cnt = 0;
                // is_bar_found_confirmed = 0;
                // is_qr_found_cnt = 0;
                // is_qr_found_confirmed = 0;            
            }
            geometry_msgs::PoseStamped hold_msg;
            if (is_blue_found ||
                ros::Time::now().toSec() - time_last < time_between_tolerance)
            {
                if (is_blue_found)
                hold_msg.pose.position.x = (0.5 - blue_y)*2; // 0.5是相机的水平视野范围
                if (is_blue_found)
                hold_msg.pose.position.y = (0.5 - blue_x)*2; // 0.5是相机的垂直视野范
                hold_msg.pose.position.z = 1.0;
            }
            else
            {
                hold_msg.pose.position.z = 0.0;
            }
            cout << "is_hold: " << hold_msg.pose.position.z << endl;
            hold_pub.publish(hold_msg);
        }
        loop_rate.sleep();
    }

    return 0;
}